#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import socket
import subprocess
import time
from customized_msgs.msg import GcsCmd  # 导入自定义消息类型 GcsCmd
from collections import deque


class TcpSenderNode(Node):
    DEBUG_MODE = False  # 可以通过修改这里的标志来控制调试模式
    # DEBUG_MODE = True  # 可以通过修改这里的标志来控制调试模式

    # DEFAULT_TARGET_IP = '192.168.2.111'  # 目标设备 IP 地址
    #DEFAULT_TARGET_IP = '192.168.51.69'  # 目标设备 IP 地址
    DEFAULT_TARGET_IP='192.168.6.6'
    DEFAULT_TARGET_PORT = 12347  # 目标设备端口
    SOCKET_TIMEOUT = 1  # 超时时间（秒）
    RETRY_LIMIT = 1  # 重试次数
    start_time = time.time()  # 记录程序启动时间
    
    message_buffer = deque()  # 用于存储待发送的消息

    message_buffer_max_size = 50

    last_ping_success_time = 0

    #msg.cmd=0
    def __init__(self):
        super().__init__('tcp_sender_node')

        # 初始化参数
        self.declare_parameter('target_ip', self.DEFAULT_TARGET_IP)
        self.declare_parameter('target_port', self.DEFAULT_TARGET_PORT)
        self.declare_parameter('DEBUG_MODE', self.DEBUG_MODE)
        self.target_ip = self.get_parameter('target_ip').get_parameter_value().string_value
        self.target_port = self.get_parameter('target_port').get_parameter_value().integer_value
        
        self.DEBUG_MODE = self.get_parameter('DEBUG_MODE').get_parameter_value().bool_value
        
        # 创建订阅者，监听 GcsCmd 消息
        self.subscriber = self.create_subscription(GcsCmd, '/winch_wireless_server/gcs_cmd', self.message_callback, 10)

        if self.DEBUG_MODE:
            # 设置定时器周期，定时发送消息
            self.timer = self.create_timer(0.2, self.debug_timer_callback)
        self.s = None
        self.get_logger().info(f"TCP sender node starting. Sending on {self.target_ip}:{self.target_port}")
        self.num=0
        self.last_ping_time=0
        self.last_ping_flag=False
        self.connect_time_tick=0.2
        self.last_retry_connect_time=time.time()
        self.last_connect_flag=False
        self.try_to_connect()
        

    def try_to_connect(self):
        if time.time()-self.last_retry_connect_time< self.connect_time_tick:
            return self.last_connect_flag
        self.s = self.create_tcp_socket()
        self.get_logger().info(f"Trying to connect {self.target_ip}:{self.target_port}")

        try:
            self.s.connect((self.target_ip, self.target_port))
            self.last_connect_flag=True
            return True
        except (socket.timeout, socket.error) as e:
            self.get_logger().error(f"Failed to connect to {self.target_ip}:{self.target_port}: {e}")
            self.s = None
            self.last_connect_flag=False
            return False
        finally:
           self.last_retry_connect_time = time.time()


    def create_tcp_socket(self):
        """
        创建并返回一个配置好的 TCP 套接字
        """
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)  # 设置 Keep-Alive 选项
        s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)  # 禁用 Nagle 算法
        s.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 5)  # 设置空闲 60 秒后开始发送 Keep-Alive 探测包
        s.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 5)  # 设置发送探测包的间隔为 10 秒
        s.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, self.RETRY_LIMIT)  # 设置最大重试次数为 5 次
        s.settimeout(self.SOCKET_TIMEOUT)
        return s

    def pop_extra_messages(self):
        while len(self.message_buffer) > self.message_buffer_max_size:
            self.get_logger().warn(f"Message buffer size{len(self.message_buffer)} exceeds {self.message_buffer_max_size}. Removing oldest message.")
            self.message_buffer.popleft()

    def send_message(self, message):
        """
        发送消息到目标设备，使用 TCP 套接字进行连接和数据传输
        """
        ping_flag=self.test_network(self.target_ip)

        if ping_flag == False:
            self.get_logger().warn("ping failed, storing message in queue1; ")
            self.message_buffer.append(message)
            self.pop_extra_messages()
            return False

        while len(self.message_buffer) > 0:
            ping_flag=self.test_network(self.target_ip)
            if ping_flag == False:
                return False
            temp_msg = self.message_buffer.popleft()
            if not self.send_one_message(temp_msg):
                self.get_logger().warn("Failed to send message in buffer.")
                self.message_buffer.appendleft(temp_msg)
                self.pop_extra_messages()
            time.sleep(0.01)

        
        ping_flag=self.test_network(self.target_ip)
        if ping_flag == False:
            self.get_logger().warn("ping failed, storing message in queue2; ")
            self.message_buffer.append(message)
            self.pop_extra_messages()
            return False
        else:
            if not self.send_one_message(message):
                self.get_logger().warn(f"Failed to send current message, storing it in queue{message}.")
                #self.last_ping_success_time = 0
                self.message_buffer.append(message)
                self.pop_extra_messages()
            else:
                return True

        
        


    def send_one_message(self, message):
        """
        发送消息到目标设备，使用 TCP 套接字进行连接和数据传输
        """
        retries = 0
        while retries < self.RETRY_LIMIT:

            if self.s == None:  # 套接字已关闭，尝试重新连接
                if not self.try_to_connect():
                    #self.last_ping_success_time = 0
                    return False

            try:
                # self.get_logger().info(f"Attempting to connect to {self.target_ip}:{self.target_port}")
                # self.s.connect((self.target_ip, self.target_port))

                # 将消息内容转换为字符串并编码成字节
                message_str = f"{message.cmd},{message.winch_speed};"
                
                self.s.sendall(message_str.encode('utf-8'))

                self.get_logger().info(f"Sent message: {message_str}")
               
                return True
            # except Exception as e:
               
            except (socket.timeout, socket.error) as e:
                
               
                retries += 1
                #self.last_ping_success_time = 0
                self.get_logger().warn(f"error sending message:{message}")
                self.get_logger().warn(f"Socket error: {e}. Retrying {retries}/{self.RETRY_LIMIT}")
                self.s = None  # 关闭套接字
                if retries >= self.RETRY_LIMIT:
                    self.get_logger().error("Failed to send message after several retries.")
                    return False
                time.sleep(0.01)  # 等待1秒后重试
                

    def debug_timer_callback(self):
        """
        调试定时器回调，用于定时发送调试消息
        """
        #msg = GcsCmd()
        msg = GcsCmd()
        msg.cmd=100
        # msg.cmd = 99
        msg.winch_speed = time.time() - self.start_time  # 设置winch_speed的值
        if self.num >= 255:
            self.num = 0
        self.num +=1  # 设置cmd的值
        # 调用发送消息的函数
        
        if not self.send_message(msg):
            self.get_logger().warn("Failed to send message.in debug timer")
            #self.last_ping_success_time = 0
    
    def message_callback(self, msg):
        """
        接收来自订阅者的 GcsCmd 消息并发送给目标设备
        """
        if not self.send_message(msg) :
            self.get_logger().warn("Failed to send message. in callback")
            #self.last_ping_success_time = 0


    def test_network(self,ip_address, timeout=1, count=1):
        
        ping_trust_time = 0.05

        if time.time() - self.last_ping_time < ping_trust_time:
            return self.last_ping_flag

        self.get_logger().info(f"test network: {ip_address}")
        try:
            # 执行 ping 命令
            result = subprocess.run(
                ["ping", "-c", str(count), "-W", str(timeout), ip_address],
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE,
                text=True
            )
            # 检查返回码
            if result.returncode == 0:
                self.get_logger().info(f"Ping {ip_address} success")
                self.last_ping_time = time.time()
                self.last_ping_flag=True
                return True
            else:
                self.last_ping_time = time.time()
                self.get_logger().info(f"Ping {ip_address} failed")
                self.last_ping_flag=False
                return False
        except Exception as e:
            self.get_logger().info(f"ping error : {e}")
            self.last_ping_time = time.time()
            self.last_ping_flag=False
            return False

def main(args=None):
    """
    主函数，初始化并启动 ROS 2 节点
    """
    rclpy.init(args=args)

    # 创建并启动 ROS 2 节点
    node = TcpSenderNode()

    try:
        rclpy.spin(node)  # 启动 ROS 2 节点
    except KeyboardInterrupt:
        pass
    finally:
        node.s.close()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    
    main()
